﻿using System;

namespace TrueSync.Physics2D
{
    public class RectangleShape : Shape
    {
        public TSVector2 max, min;

        public override ShapeType ShapeType
        {
            get
            {
                return ShapeType.Rectangle;
            }
        }

        public RectangleShape(TSVector2 min, TSVector2 max)
        {
            this.max = TSVector2.Max(min, max);
            this.min = TSVector2.Min(min, max);
        }

        public override Shape Clone()
        {
            return new RectangleShape(max, min);
        }

        public override bool CompareTo(Shape shape)
        {
            if (shape is RectangleShape)
            {
                RectangleShape other = (RectangleShape)shape;
                if (other.max == max && other.min == min)
                    return true;
            }

            return false;
        }

        public override void ComputeAABB(out AABB aabb, TSTransform transform)
        {
            TSVector2 min = transform.TransformPoint(this.max);
            TSVector2 max = min; 

            TSVector2 p1 = transform.TransformPoint(new TSVector2(this.max.x, this.min.y));
            min = TSVector2.Min(p1, min); max = TSVector2.Max(p1, max);

            TSVector2 p2 = transform.TransformPoint(new TSVector2(this.min.x, this.max.y));
            min = TSVector2.Min(p2, min); max = TSVector2.Max(p2, max);

            TSVector2 p3 = transform.TransformPoint(new TSVector2(this.min.x, this.min.y));
            min = TSVector2.Min(p3, min); max = TSVector2.Max(p3, max);

            aabb = new AABB(min, max);
        }

        public override bool RayCast(out RayCastOutput output, RayCastInput input, TSTransform transform)
        {
            output = new RayCastOutput();

            FP tmin = FP.MinValue;
            FP tmax = FP.MaxValue;

            TSVector2 p = transform.TransformPoint(input.Point1);
            TSVector2 d = transform.TransformPoint(input.Point2) - p;
            TSVector2 absD = TSVector2.Abs(d);

            TSVector2 normal = TSVector2.zero;

            for (int i = 0; i < 2; ++i)
            {
                FP absD_i = i == 0 ? absD.x : absD.y;
                FP lowerBound_i = i == 0 ? min.x : min.y;
                FP upperBound_i = i == 0 ? max.x : max.y;
                FP p_i = i == 0 ? p.x : p.y;

                if (absD_i < Settings.Epsilon)
                {
                    // Parallel.
                    if (p_i < lowerBound_i || upperBound_i < p_i)
                    {
                        return false;
                    }
                }
                else
                {
                    FP d_i = i == 0 ? d.x : d.y;

                    FP inv_d = FP.One / d_i;
                    FP t1 = (lowerBound_i - p_i) * inv_d;
                    FP t2 = (upperBound_i - p_i) * inv_d;

                    // Sign of the normal vector.
                    FP s = -FP.One;

                    if (t1 > t2)
                    {
                        TSMath.Swap(ref t1, ref t2);
                        s = FP.One;
                    }

                    // Push the min up
                    if (t1 > tmin)
                    {
                        if (i == 0)
                        {
                            normal.x = s;
                        }
                        else
                        {
                            normal.y = s;
                        }

                        tmin = t1;
                    }

                    // Pull the max down
                    tmax = FP.Min(tmax, t2);

                    if (tmin > tmax)
                    {
                        return false;
                    }
                }
            }

            // Does the ray start inside the box?
            // Does the ray intersect beyond the max fraction?
            if (tmin < 0 || input.MaxFraction < tmin)
            {
                return false;
            }

            // Intersection.
            output.Fraction = tmin;
            output.Normal = transform.TransformVector(normal);
            return true;
        }

        public override bool TestPoint(TSTransform transform, TSVector2 point)
        {
            TSVector2 localPoint = transform.InverseTransformPoint(point);
            if(localPoint.x > max.x || localPoint.x < min.x)
                return false;
            if (localPoint.y > max.y || localPoint.y < min.y)
                return false;
            return true; 
        }
    }
}
